<%
if !defined?(topic_prefix) || topic_prefix == nil || topic_prefix.empty?
  $topic_prefix = ''
else
  $topic_prefix = topic_prefix
end
%>

<?xml version="1.0" ?>
<sdf version='1.9'>
  <model name="suction_gripper">
    <link name='end_effector'>
      <inertial>
        <mass>2</mass>
        <inertia>
          <ixx>0.00166667</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.00166667</iyy>
          <iyz>0</iyz>
          <izz>0.00166667</izz>
         </inertia>
      </inertial>
      <collision name='end_effector_collision'>
        <pose>0.025 0 0 0 1.57079 0</pose>
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.05</length>
          </cylinder>
        </geometry>
      </collision>
      <visual name='end_effector_visual'>
        <pose>0.025 0 0 0 1.57079 0</pose>
        <geometry>
          <cylinder>
            <radius>0.05</radius>
            <length>0.05</length>
          </cylinder>
        </geometry>
        <material>
          <diffuse>0.05 0.05 0.05</diffuse>
          <specular>0.8 0.8 0.8</specular>
        </material>
      </visual>
    </link>
    <link name='suction'>
      <inertial>
        <mass>2</mass>
        <inertia>
          <ixx>0.00166667</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.00166667</iyy>
          <iyz>0</iyz>
          <izz>0.00166667</izz>
         </inertia>
      </inertial>

      <collision name='suction_collision_11'>
        <pose>0.0505 0 0 0 1.57079 0</pose>
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.01</length>
          </cylinder>
        </geometry>
      </collision>
      <visual name='suction_visual_11'>
        <pose>0.0505 0 0 0 1.57079 0</pose>
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.01</length>
          </cylinder>
        </geometry>
        <material>
          <diffuse>0.02 0.02 0.02</diffuse>
          <specular>0.3 0.3 0.3</specular>
        </material>
      </visual>
      <sensor name='sensor_contact_11' type='contact'>
        <contact>
          <collision>suction_collision_11</collision>
          <topic><%= $topic_prefix %>/gripper/contact_sensor_11</topic>
        </contact>
        <always_on>1</always_on>
        <update_rate>100</update_rate>
      </sensor>

      <collision name='suction_collision_10'>
        <pose>0.0505 0 -0.04 0 1.57079 0</pose>
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.01</length>
          </cylinder>
        </geometry>
      </collision>
      <visual name='suction_visual_10'>
        <pose>0.0505 0 -0.04 0 1.57079 0</pose>
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.01</length>
          </cylinder>
        </geometry>
        <material>
          <diffuse>0.02 0.02 0.02</diffuse>
          <specular>0.3 0.3 0.3</specular>
        </material>
      </visual>
      <sensor name='sensor_contact_10' type='contact'>
        <contact>
          <collision>suction_collision_10</collision>
          <topic><%= $topic_prefix %>/gripper/contact_sensor_10</topic>
        </contact>
        <always_on>1</always_on>
        <update_rate>100</update_rate>
      </sensor>

      <collision name='suction_collision_12'>
        <pose>0.0505 0 0.04 0 1.57079 0</pose>
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.01</length>
          </cylinder>
        </geometry>
      </collision>
      <visual name='suction_visual_12'>
        <pose>0.0505 0 0.04 0 1.57079 0</pose>
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.01</length>
          </cylinder>
        </geometry>
        <material>
          <diffuse>0.02 0.02 0.02</diffuse>
          <specular>0.3 0.3 0.3</specular>
        </material>
      </visual>
      <sensor name='sensor_contact_12' type='contact'>
        <contact>
          <collision>suction_collision_12</collision>
          <topic><%= $topic_prefix %>/gripper/contact_sensor_12</topic>
        </contact>
        <always_on>1</always_on>
        <update_rate>100</update_rate>
      </sensor>

      <collision name='suction_collision_01'>
        <pose>0.0505 -0.04 0 0 1.57079 0</pose>
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.01</length>
          </cylinder>
        </geometry>
      </collision>
      <visual name='suction_visual_01'>
        <pose>0.0505 -0.04 0 0 1.57079 0</pose>
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.01</length>
          </cylinder>
        </geometry>
        <material>
          <diffuse>0.02 0.02 0.02</diffuse>
          <specular>0.3 0.3 0.3</specular>
        </material>
      </visual>
      <sensor name='sensor_contact_01' type='contact'>
        <contact>
          <collision>suction_collision_01</collision>
          <topic><%= $topic_prefix %>/gripper/contact_sensor_01</topic>
        </contact>
        <always_on>1</always_on>
        <update_rate>100</update_rate>
      </sensor>

      <collision name='suction_collision_21'>
        <pose>0.0505 0.04 0 0 1.57079 0</pose>
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.01</length>
          </cylinder>
        </geometry>
      </collision>
      <visual name='suction_visual_21'>
        <pose>0.0505 0.04 0 0 1.57079 0</pose>
        <geometry>
          <cylinder>
            <radius>0.02</radius>
            <length>0.01</length>
          </cylinder>
        </geometry>
        <material>
          <diffuse>0.02 0.02 0.02</diffuse>
          <specular>0.3 0.3 0.3</specular>
        </material>
      </visual>
      <sensor name='sensor_contact_21' type='contact'>
        <contact>
          <collision>suction_collision_21</collision>
          <topic><%= $topic_prefix %>/gripper/contact_sensor_21</topic>
        </contact>
        <always_on>1</always_on>
        <update_rate>100</update_rate>
      </sensor>

    </link>

    <joint name='suction_joint' type='fixed'>
      <parent>end_effector</parent>
      <child>suction</child>
    </joint>

    <!-- Controller -->
    <plugin
      filename="libSuctionGripper.so"
      name="mbzirc::SuctionGripperPlugin">
      <parent_link>suction</parent_link>
      <contact_sensor_topic_prefix><%= $topic_prefix %>/gripper</contact_sensor_topic_prefix>
      <command_topic><%= $topic_prefix %>/gripper/suction_on</command_topic>
    </plugin>

    <frame name="mount_point"/>

  </model>
</sdf>
